#include <cppad/cppad.hpp>
#include <cppad/ipopt/solve.hpp>
#include <cmath>
#include "Eigen/Core"
#include "Eigen/LU"
#include "Eigen/QR"

#include "mpccbf.h"
#include "parameters.h"

using CppAD::AD;

std::size_t x_start;
std::size_t y_start;
std::size_t yaw_start;
std::size_t v_x_start;
std::size_t v_y_start;
std::size_t delta_start;
std::size_t acc_x_start;

std::size_t acc_y_start;

namespace mpccbf {
    class FG_eval {
        public:
            Eigen::MatrixXd matrix_r;  //rqp 控制输入的成本权重、状态误差的成本权重和目标点误差的成本权重
            Eigen::MatrixXd matrix_q;
            Eigen::MatrixXd matrix_p;
            TrajectoryPoint target_point;
            TrajectoryPoint nearest_point;

            SystemState state;

            int horizon_;
            double dt;
            double gamma;
            FG_eval(Eigen::MatrixXd matrix_r, Eigen::MatrixXd matrix_q, Eigen::MatrixXd matrix_p,
                TrajectoryPoint target_point, 
                TrajectoryPoint nearest_point, SystemState state, int horizon_, double dt, double gamma)
             {
            this->matrix_r = matrix_r;
            this->matrix_q = matrix_q;
            this->matrix_p = matrix_p;
            this->target_point = target_point;

            this->nearest_point = nearest_point;
            this->state = state;
            this->horizon_ = horizon_;
            this->dt = dt;
            this->gamma = gamma;
        }
            
            typedef CPPAD_TESTVECTOR(AD<double>) ADvector;
            void operator()(ADvector& fg, const ADvector& vars) {
                fg[0] = 0;
             
            for (int t = 0; t < horizon_; ++t) {
        fg[0] += matrix_p(0,0) * CppAD::pow(vars[x_start + t] - target_point.x, 2);
        fg[0] += matrix_p(1,1) * CppAD::pow(vars[y_start + t] - target_point.y, 2);
        fg[0] += matrix_p(2,2) * CppAD::pow(vars[v_x_start + t] - target_point.v_x, 2);
        fg[0] += matrix_p(3,3) * CppAD::pow(vars[v_y_start + t] - target_point.v_y, 2);
        fg[0] += matrix_p(4,4) * CppAD::pow(vars[yaw_start + t] - target_point.heading, 2);
    }

    // 计算状态误差的代价
    for (int t = 0; t < horizon_; ++t) {
        fg[0] += matrix_q(0,0) * CppAD::pow(vars[x_start + t] - nearest_point.x, 2);
        fg[0] += matrix_q(1,1) * CppAD::pow(vars[y_start + t] - nearest_point.y, 2);
        fg[0] += matrix_q(2,2) * CppAD::pow(vars[v_x_start + t] - nearest_point.v_x, 2);
        fg[0] += matrix_q(3,3) * CppAD::pow(vars[v_y_start + t] - nearest_point.v_y, 2);
        fg[0] += matrix_q(4,4) * CppAD::pow(vars[yaw_start + t] - nearest_point.heading, 2);
    }

    // 计算控制输入的代价
    for (int t = 0; t < horizon_ - 1; ++t) {
        fg[0] += matrix_r(0,0) * CppAD::pow(vars[delta_start + t], 2);
        fg[0] += matrix_r(1,1) * CppAD::pow(vars[acc_y_start + t], 2);
        fg[0] += matrix_r(2,2) * CppAD::pow(vars[acc_x_start + t], 2);
    }
                fg[1+x_start] = vars[x_start];
                fg[1+y_start] = vars[y_start];
                fg[1+v_x_start] = vars[v_x_start];
                fg[1+v_y_start] = vars[v_y_start];
                fg[1+yaw_start] = vars[yaw_start];

                for(int t = 1;t < horizon_;++t){
                    // The state at time t+1 .
                    AD<double> x1 = vars[x_start + t];
                    AD<double> y1 = vars[y_start + t];
                    AD<double> v_x_1 = vars[v_x_start + t];
                    AD<double> v_y_1 = vars[v_y_start + t];
                    AD<double> yaw1 = vars[yaw_start + t];

                    // The state at time t.
                    AD<double> x0 = vars[x_start + t - 1];
                    AD<double> y0 = vars[y_start + t - 1];
                    AD<double> v_x_0 = vars[v_x_start + t - 1]; 
                    AD<double> v_y_0 = vars[v_y_start + t - 1];
                    AD<double> yaw0 = vars[yaw_start + t - 1];

                    AD<double> delta0 = vars[delta_start + t -1];
                    AD<double> acc_x_0 = vars[acc_x_start + t -1];
                    AD<double> acc_y_0 = vars[acc_y_start + t -1];

                    AD<double> d_next = CppAD::pow((vars[x_start + t] - state.obs_x), 2) + CppAD::pow((vars[y_start + t] - state.obs_y), 2);
                    AD<double> d = CppAD::pow((vars[x_start + t - 1] - state.obs_x), 2) + CppAD::pow((vars[y_start + t - 1] - state.obs_y), 2);

                    fg[1 + x_start + t] = x1 - (x0 +(v_x_0 * cos(yaw0) - v_y_0 * sin(yaw0))* dt);
                    fg[1 + y_start + t] = y1 - (y0 +(v_x_0 * sin(yaw0) + v_y_0 * cos(yaw0))* dt);
                    fg[1 + yaw_start + t] = yaw1 - (yaw0 +delta0 * dt);
                    fg[1 + v_x_start + t] = v_x_1 - (v_x_0 +acc_x_0 * dt);
                    fg[1 + v_y_start + t] = v_y_1 - (v_y_0 +acc_y_0 * dt);
                    
                  
                }

            }
    };

    MPCCBF::MPCCBF() {

        ts_ = 0.05;
        CBF_Gamma = 0.1;

        Max_Acceleration = 20.0;
        Max_UnitTurningAngle = M_PI ;

        Max_X_Y_Range = 1000000;
        Max_Velocity = 30.0;
        Max_YawRange = 2.0 * M_PI;

        matrix_state_ = Matrix::Zero(state_size_ , 1);
        matrix_state_ref_ = Matrix::Zero(state_size_ , 1);

        matrix_r_ = Matrix::Identity(controls_, controls_);
   	 matrix_r_(0,0) = 1;    //角度控制量        
        matrix_r_(1,1) = 1;    //acc_x
        matrix_r_(2,2) = 1;    //acc_y


       matrix_p_ = Matrix::Zero(state_size_, state_size_);
       matrix_p_(0,0) = 10;  //x
       matrix_p_(1,1) = 10;  //y
       matrix_p_(2,2) = 5;  //vx
       matrix_p_(3,3) = 5;  //vy
       matrix_p_(4,4) = 10;  //theta

        matrix_q_ = Matrix::Zero(state_size_, state_size_);
        matrix_q_(0,0) = 80;
        matrix_q_(1,1) = 100;
        matrix_q_(2,2) = 5;
        matrix_q_(3,3) = 5;
        matrix_q_(4,4) = 5;


        x_start = 0;                              //0
        y_start = x_start + horizon_;             //5
        v_x_start = y_start + horizon_;           //10
        v_y_start = v_x_start + horizon_;         //15
        yaw_start = v_y_start + horizon_;         //20

        delta_start = yaw_start + horizon_;       //25
        acc_x_start = delta_start + horizon_ - 1; //29
        acc_y_start = acc_x_start + horizon_ - 1; //33

    }
    MPCCBF::~MPCCBF() {}

    std::vector<double> MPCCBF::Solve(const SystemState state, std::vector<TrajectoryPoint> planning_trajectoryPoint, const TrajectoryPoint target_point)
    {
        typedef CPPAD_TESTVECTOR(double) Dvector;

        double x = state.x;
        double y = state.y;
        double theta = state.yaw;
        double vx_ = state.v_x; 
        double vy_ = state.v_y;

        size_t n_vars = state_size_ * horizon_ + controls_ * (horizon_ - 1);//37
        size_t n_constraints = state_size_ * horizon_ + (horizon_ - 1); //29

        Dvector vars(n_vars);
        for(int i=0; i< n_vars; ++i){
            vars[i] = 0;
        }

        vars[x_start] = x;
        vars[y_start] = y;
        vars[v_x_start] = vx_;
        vars[v_y_start] = vy_;
        vars[yaw_start] = theta;

        Dvector vars_lowerbound(n_vars);
        Dvector vars_upperbound(n_vars);

        for (int i = 0; i < v_x_start; i++) {
            vars_upperbound[i] = Max_X_Y_Range;
            vars_lowerbound[i] = -Max_X_Y_Range;
        }   
        for (int i = v_x_start; i < yaw_start; ++i){
            vars_upperbound[i] = Max_Velocity;
            vars_lowerbound[i] = -Max_Velocity;
        }
        for (int i = yaw_start; i < delta_start; ++i){
            vars_upperbound[i] = Max_YawRange;
            vars_lowerbound[i] = -Max_YawRange;
        }
        for (int i = delta_start; i < acc_x_start; ++i){
            vars_upperbound[i] = Max_UnitTurningAngle;
            vars_lowerbound[i] = -Max_UnitTurningAngle;
        }
        for (int i = acc_x_start; i < n_vars; ++i){
            vars_upperbound[i] = Max_Acceleration;
            vars_lowerbound[i] = -Max_Acceleration;            
        }

        Dvector constraints_upperbound(n_constraints);
        Dvector constraints_lowerbound(n_constraints);
        for (int i = 0; i < state_size_ * horizon_; i++) {
            constraints_upperbound[i] = 0;
            constraints_lowerbound[i] = 0;
        }

        constraints_upperbound[x_start] = x;
        constraints_upperbound[y_start] = y;
        constraints_upperbound[v_x_start] = vx_;
        constraints_upperbound[v_y_start] = vy_;
        constraints_upperbound[yaw_start] = theta;

        constraints_lowerbound[x_start] = x;
        constraints_lowerbound[y_start] = y;
        constraints_lowerbound[v_x_start] = vx_;
        constraints_lowerbound[v_y_start] = vy_;
        constraints_lowerbound[yaw_start] = theta;

        for(int i = state_size_ * horizon_; i < n_constraints; ++i){
            constraints_upperbound[i] = 1.0e19;
            constraints_lowerbound[i] = 0.0;
        }
        TrajectoryPoint nearest_point = QueryNearestPointByPos(state.x, state.y, planning_trajectoryPoint); ;
        FG_eval fg_eval(matrix_r_, matrix_q_, matrix_p_, target_point, nearest_point, state, horizon_, ts_, CBF_Gamma);

        std::string options;
        options += "Integer print_level  0\n";
        options += "Sparse  true        forward\n";
        options += "Sparse  true        reverse\n";
        options += "Numeric max_cpu_time          0.5\n";  

        CppAD::ipopt::solve_result<Dvector> solution;
        CppAD::ipopt::solve<Dvector,FG_eval>(
            options,vars,vars_lowerbound,vars_upperbound,constraints_lowerbound,
            constraints_upperbound, fg_eval, solution);
        
        bool sol_res = solution.status == CppAD::ipopt::solve_result<Dvector>::success;

        std::vector<double> result;
        if(sol_res){
            result.push_back(solution.x[delta_start]);
            result.push_back(solution.x[acc_x_start]);
            result.push_back(solution.x[acc_y_start]);
            result.push_back(solution.obj_value);
        
        }
        return result;
    }

    TrajectoryPoint MPCCBF::QueryNearestPointByPos(const double x, const double y, std::vector<TrajectoryPoint> trajectory_points_){
        double d_min = MPCCBF::PointDistanceSquare(trajectory_points_.front(), x, y);
        std::size_t index_min = 0;
        for(std::size_t i = 1; i < trajectory_points_.size(); ++i){
            double d_temp = MPCCBF::PointDistanceSquare(trajectory_points_[i], x, y);
            if (d_temp < d_min) {
                d_min = d_temp;
                index_min = i;
            }
        }
        return trajectory_points_[index_min];
    }

    double MPCCBF::PointDistanceSquare(const TrajectoryPoint &point, const double x, const double y) {
        const double dx = point.x - x;
        const double dy = point.y - y;
        return dx * dx + dy * dy;
    }

    /*-2pi-2pi归一化到-pi- pi*/
    double MPCCBF::NormalizeAngle(const double angle) {
        double a = std::fmod(angle + M_PI, 2.0 * M_PI);
        if (a < 0.0) {
            a += (2.0 * M_PI);
        }
        return a - M_PI;
    }
}

